//学习教程 https://wiki.seeedstudio.com/cn/Getting_Started/
#include "include.h"
#include "SerialServo.h"
#include <Arduino.h>
#include <ArduinoJson.h>
#include <WiFi.h>
#include <Wire.h>
// 需要此库以进行低级别访问设置
#include <HardwareSerial.h>
#include <PubSubClient.h>

// 定义两个串口设备，映射到两个内部UART
HardwareSerial MySerial0(0);
HardwareSerial MySerial1(1);


#define NTP1 "ntp1.aliyun.com"
#define NTP2 "ntp2.aliyun.com"
#define NTP3 "ntp3.aliyun.com"
//填写WIFI入网信息
const char* ssid = "bearpi";             // WIFI账户
const char* password = "12345678";       // WIFI密码
const char* mqtt_server = "42.193.218.152"; // MQTT服务器IP地址    
//const char* mqtt_server = "192.168.43.191"; // MQTT服务器IP地址    
WiFiClient espClient;
PubSubClient client(espClient);

long lastMsg = 0;
char msg[50];
int value = 0;
char send_message[200] = "";
int message_count = 0;

int position_array[5] = {500,500,500,500,500};
int temperature_array[5] = {0,0,0,0,0};
int volatge_array[5] = {0,0,0,0,0};

int value_can_read_flag = 0;
int value_1 = 487;
int value_2 = 937;
int value_3 = 841;
int value_4 = 814;
int value_5 = 500;
int temp[6] = {2,487,937,841,814,500};

int is_Lock = 2;
int clock_count = 0;

// const char* mqtt_username = "admin";     // MQTT用户名
// const char* mqtt_password = "123456";    // MQTT密码
// const char* mqtt_client_id = "esp32_c3"; // MQTT客户端ID
const String WDAY_NAMES[] = {"星期天", "星期一", "星期二", "星期三", "星期四", "星期五", "星期六"};                //星期
const String MONTH_NAMES[] = {"Jan", "Feb", "Mar", "Apr", "May", "Jun", "Jul", "Aug", "Sep", "Oct", "Nov", "Dec"}; //月份

//time_t now; //实例化时间
void setClock()
{

  struct tm timeInfo; //声明一个结构体
  if (!getLocalTime(&timeInfo))
  { //一定要加这个条件判断，否则内存溢出
    printf("Failed to obtain time");
    return;
  }
  //Serial.print(asctime(&timeInfo)); //默认打印格式：Mon Oct 25 11:13:29 2021
  String date = WDAY_NAMES[timeInfo.tm_wday];
  //printf(date.c_str());
  // sprintf_P(buff1, PSTR("%04d-%02d-%02d %s"), timeInfo.tm_year + 1900, timeInfo.tm_mon + 1, timeInfo.tm_mday, WDAY_NAMES[timeInfo.tm_wday].c_str());
  String shuju = String(timeInfo.tm_year + 1900); //年
  shuju += "-";
  shuju += timeInfo.tm_mon + 1; //月
  shuju += "-";
  shuju += timeInfo.tm_mday; //日
  shuju += " ";
  shuju += timeInfo.tm_hour; //时
  shuju += ":";
  shuju += timeInfo.tm_min;
  shuju += ":";
  shuju += timeInfo.tm_sec;
  shuju += " ";
  shuju += WDAY_NAMES[timeInfo.tm_wday].c_str(); //星期
  shuju += "\r\n";
  printf(shuju.c_str());

  for (int mysql_id = 0; mysql_id < 5; mysql_id++)
  {
    sprintf(send_message, "{\"time_date\":\"%s\",\"id\":%d,\"temperature\":%d,\"volatge\":%.03f}",\
    shuju.c_str(),mysql_id+1,temperature_array[mysql_id],(float)volatge_array[mysql_id]*0.001);
    client.publish("to_mysql", send_message);
    memset(send_message, 0, sizeof(send_message));
  }

}

void setup_wifi() {
    delay(10);
    // We start by connecting to a WiFi network
    Serial.println();
    Serial.print("Connecting to ");
    Serial.println(ssid);
  
    WiFi.begin(ssid, password);
  
    while (WiFi.status() != WL_CONNECTED) {
      delay(500);
      Serial.print(".");
    }
  
    Serial.println("");
    Serial.println("WiFi connected");
    Serial.println("IP address: ");
    Serial.println(WiFi.localIP());
}

  const char* json;
  void callback(char* topic, byte* payload, unsigned int length) {
  
    Serial.print("Message arrived [");
    Serial.print(topic);
    Serial.print("] ");
    for (int i = 0; i < length; i++) {
      Serial.print((char)payload[i]);
    }
    Serial.println();
    json = (char*)payload;
    // 创建一个 JSON 对象
    StaticJsonDocument<200> doc;

    // 解析 JSON 字符串
    DeserializationError error = deserializeJson(doc, json);
    
    if (error) {
        Serial.print(F("deserializeJson() failed: "));
        Serial.println(error.f_str());
        return;
    }
    temp[0]=is_Lock;
    is_Lock = doc["is_Lock"];//获取值
    if (is_Lock==0)
    {
      is_Lock = temp[0];  
    }
    
    value_can_read_flag++;
    if(value_can_read_flag==1)
    {
      temp[1] = value_1;
      temp[2] = value_2;
      temp[3] = value_3;
      temp[4] = value_4;  
      temp[5] = value_5;
          // 提取数据
      value_1 = doc["unity_S1"];
      value_2 = doc["unity_S2"];
      value_3 = doc["unity_S3"];
      value_4 = doc["unity_S4"];
      value_5 = doc["unity_S5"];

      if(value_1==0){value_1 = temp[1];}
      if(value_2==0){value_2 = temp[2];}
      if(value_3==0){value_3 = temp[3];}
      if(value_4==0){value_4 = temp[4];}
      if(value_5==0){value_5 = temp[5];}

      // Serial.printf("unity_S1: %d\r\n",value_1);
      // Serial.printf("unity_S2: %d\r\n",value_2);
      // Serial.printf("unity_S3: %d\r\n",value_3);
      // Serial.printf("unity_S4: %d\r\n",value_4);
      // Serial.printf("unity_S5: %d\r\n",value_5);

      value_can_read_flag = 0;
    }


    // //数据处理
    // if (strstr((char*)payload, "message_on") != NULL) {
    //     printf("message_on\r\n");
    // } else {
    //     printf("message_off\r\n");
    // }
  
}
  
/*
void callback(const char* topic, byte* payload, unsigned int length) {

  printf("On message....");
  JsonDocument doc;
  char json[length + 1];
  strncpy (json, (char*)payload, length);
  json[length] = '\0';
  printf("Topic: ");
  printf(topic);
  printf("Message: ");
  printf(json);

  DeserializationError error = deserializeMsgPack(doc, json);

  // Test if parsing succeeded
  if (error) {
    printf("deserializeMsgPack() failed: ");
    return;
  }
  // Check request method，
  //----如果有新的数据内容解析，类似添加即可----
  String unity_P4_string= String((const char*)doc["unity_S4"]);

  // if(unity_P4_string=="500")
  // {
  //   printf("message_on\r\n");
  //   LobotSerialServoMove(MySerial0,1,500,2000);//加大逆时针旋转从上看舵盘
  // }
  // if(unity_P4_string=="800")
  // {
  //   printf("message_on\r\n");
  //   LobotSerialServoMove(MySerial0,1,800,2000);//加大逆时针旋转从上看舵盘
  // }

  // int unity_P4_int;
  // printf("unity_s4_int: %d\r\n", unity_P4_int);
  // if(unity_P4_int<850&&unity_P4_int>150)
  // {
  //   LobotSerialServoMove(MySerial0,4,unity_P4_int,2000);
  // }
}
*/


void reconnect() {
    // Loop until we're reconnected
    while (!client.connected()) {
      Serial.print("Attempting MQTT connection...");
      // Attempt to connect
      if (client.connect("XIAO_ESP32")) {
        Serial.println("connected");
        // Subscribe
        client.subscribe("XIAO_ESP32/LEDOUTPUT");
      } else {
        Serial.print("failed, rc=");
        Serial.print(client.state());
        Serial.println(" try again in 5 seconds");
        // Wait 5 seconds before retrying
        delay(5000);
      }
    }
}


void setup()
{
    // 对于USB，正常使用Serial：
    Serial.begin(115200);

    // 配置MySerial0，TX=D6，RX=D7（-1, -1表示使用默认设置）
    MySerial0.begin(115200, SERIAL_8N1, -1, -1);
    MySerial0.print("MySerial0");

    // 配置MySerial1，RX=D9，TX=D10
    MySerial1.begin(115200, SERIAL_8N1, 9, 10);
    MySerial1.print("MySerial1");

    // 连接WIFI
    setup_wifi();
    client.setServer(mqtt_server, 1883);
    Wire.begin();
    configTime(8 * 3600, 0, NTP1, NTP2, NTP3);


    client.subscribe("XIAO_ESP32/LEDOUTPUT");
    client.setCallback(callback);

    //角度初始化->读取角度再次确认->读取温度电压
    // for(int angle_id=1;angle_id<=5;angle_id++)
    // {
    //     LobotSerialServoMove(MySerial0,angle_id,90,2000);
    //     delay(1000);
    // }
    //emqx连接、初始化状态上报

    
    //点亮led灯

    //执行3次下面语句,for循环,判断是否正确下载本次程序
    for(int nuber_start=0;nuber_start<3;nuber_start++)
    {
        printf("xiao esp32_c3 --> download_00002\r\n");
        delay(1000);
    }

    // LobotSerialServoMove(MySerial0,1,487,2000);//加大逆时针旋转从上看舵盘
    // LobotSerialServoMove(MySerial0,2,937,2000);
    // LobotSerialServoMove(MySerial0,3,841,2000);
    // LobotSerialServoMove(MySerial0,4,814,2000);
    // LobotSerialServoMove(MySerial0,5,500,2000);
    // delay(3500);

    for(int action_time=0;action_time<2;action_time++)
    {
        LobotSerialServoMove(MySerial0,1,487,2000);//加大逆时针旋转从上看舵盘
        LobotSerialServoMove(MySerial0,2,500,2000);
        LobotSerialServoMove(MySerial0,3,500,2000);
        LobotSerialServoMove(MySerial0,4,500,2000);
        LobotSerialServoMove(MySerial0,5,500,2000);
        delay(3500);
        
        LobotSerialServoMove(MySerial0,1,487,2000);//加大逆时针旋转从上看舵盘
        LobotSerialServoMove(MySerial0,2,937,2000);
        LobotSerialServoMove(MySerial0,3,841,2000);
        LobotSerialServoMove(MySerial0,4,814,2000);
        LobotSerialServoMove(MySerial0,5,500,2000);
        delay(3500);
    }
    LobotSerialServoUnload(MySerial0,1);
    LobotSerialServoUnload(MySerial0,2);
    LobotSerialServoUnload(MySerial0,3);
    LobotSerialServoUnload(MySerial0,4);
    LobotSerialServoUnload(MySerial0,5);
    
}



void loop()
{


    /*---------- 基础功能 ----------*/
    //获取网络时间 上报数据库
    //setClock();
    //网络连接
    if (!client.connected()) {
        reconnect();
      }
      client.loop();
    //订阅主题，控制舵机角度转速，上电状态

    // sprintf(send_message, "Hello, world #%d", message_count);
    // client.publish("XIAO_ESP32C3_Telemetry/Temperaturedataread", send_message);
    // message_count++;
    // if (message_count > 10) {
    //     message_count = 0;
    // }


    if(is_Lock == 2)
    {
      LobotSerialServoUnload(MySerial0,1);
      LobotSerialServoUnload(MySerial0,2);
      LobotSerialServoUnload(MySerial0,3);
      LobotSerialServoUnload(MySerial0,4);
      LobotSerialServoUnload(MySerial0,5);

      for (int i = 0; i < 5; i++)
      {
          position_array[i] = LobotSerialServoReadPosition(MySerial0,i+1);
      }
  
      // printf("position_array[1]=%d\r\n",position_array[0]);
      // printf("position_array[2]=%d\r\n",position_array[1]);
      // printf("position_array[3]=%d\r\n",position_array[2]);
      // printf("position_array[4]=%d\r\n",position_array[3]);
      // printf("position_array[5]=%d\r\n",position_array[4]);
      //P1
      position_array[0] = (int)(((float)(position_array[0]-487))/4.17)*(-1);
      sprintf(send_message, "%d", position_array[0]);
      client.publish("unity_P1", send_message);
      memset(send_message, 0, sizeof(send_message));
      // printf("position_array[1]=%d\r\n",position_array[0]);
      //P2
      position_array[1] = (int)(((float)(position_array[1]-500))/4.17)*(-1);
      sprintf(send_message, "%d", position_array[1]);
      client.publish("unity_P2", send_message);
      memset(send_message, 0, sizeof(send_message));
      // printf("position_array[2]=%d\r\n",position_array[1]);
  
      //P3
      position_array[2] = (int)(((float)(position_array[2]-500))/4.17);
      sprintf(send_message, "%d", position_array[2]);
      client.publish("unity_P3", send_message);
      memset(send_message, 0, sizeof(send_message));
      // printf("position_array[3]=%d\r\n",position_array[2]);
  
      //P4
      position_array[3] = (int)(((float)(position_array[3]-500))/4.17);
      sprintf(send_message, "%d", position_array[3]);
      client.publish("unity_P4", send_message);
      memset(send_message, 0, sizeof(send_message));
      delay(20);


      for (int i = 0; i < 5; i++)
      {
          temperature_array[i] = LobotSerialServoReadTemp(MySerial0,i+1);
      }
     
      sprintf(send_message, "%d", temperature_array[0]);
      client.publish("unity_st1", send_message);
      memset(send_message, 0, sizeof(send_message));
      //printf("temperature_array[0]=%d\r\n",temperature_array[0]);
  
      sprintf(send_message, "%d", temperature_array[1]);
      client.publish("unity_st2", send_message);
      memset(send_message, 0, sizeof(send_message));
      //printf("temperature_array[1]=%d\r\n",temperature_array[1]);
  
      sprintf(send_message, "%d", temperature_array[2]);
      client.publish("unity_st3", send_message);
      memset(send_message, 0, sizeof(send_message));
      //printf("temperature_array[2]=%d\r\n",temperature_array[2]);
  
      sprintf(send_message, "%d", temperature_array[3]);
      client.publish("unity_st4", send_message);
      memset(send_message, 0, sizeof(send_message));
      //printf("temperature_array[3]=%d\r\n",temperature_array[3]);
  
      sprintf(send_message, "%d", temperature_array[4]);
      client.publish("unity_st5", send_message);
      memset(send_message, 0, sizeof(send_message));
      //printf("temperature_array[4]=%d\r\n",temperature_array[4]);
      delay(40);
  
      for (int i = 0; i < 5; i++)
      {
          volatge_array[i] = LobotSerialServoReadVin(MySerial0,i+1);
      }
  
      sprintf(send_message, "%.03f", (float)volatge_array[0]*0.001);
      client.publish("unity_sv1", send_message);
      memset(send_message, 0, sizeof(send_message));
  
      sprintf(send_message, "%.03f", volatge_array[1]*0.001);
      client.publish("unity_sv2", send_message);
      memset(send_message, 0, sizeof(send_message));
  
      sprintf(send_message, "%.03f", volatge_array[2]*0.001);
      client.publish("unity_sv3", send_message);
      memset(send_message, 0, sizeof(send_message));
  
      sprintf(send_message, "%.03f", volatge_array[3]*0.001);
      client.publish("unity_sv4", send_message);
      memset(send_message, 0, sizeof(send_message));
  
      sprintf(send_message, "%.03f", volatge_array[4]*0.001);
      client.publish("unity_sv5", send_message);
      memset(send_message, 0, sizeof(send_message));
      delay(20);

      clock_count++;
      if(clock_count == 50)
      {
        setClock();
        clock_count = 0;
      }
    }
    // printf("position_array[4]=%d\r\n",position_array[3]);


    // LobotSerialServoMove(MySerial0,2,937,2000);
    // LobotSerialServoMove(MySerial0,3,841,2000);
    // LobotSerialServoMove(MySerial0,4,814,2000);
    // LobotSerialServoMove(MySerial0,5,500,2000);
  


    // sprintf(send_message, "%d", position_array[1]);
    // client.publish("unity_P2", send_message);
    // memset(send_message, 0, sizeof(send_message));
    // sprintf(send_message, "%d", position_array[2]);
    // client.publish("unity_P3", send_message);
    // memset(send_message, 0, sizeof(send_message));
    // sprintf(send_message, "%d", position_array[3]);
    // client.publish("unity_P4", send_message);
    // memset(send_message, 0, sizeof(send_message));
    // sprintf(send_message, "%d", position_array[4]);
    // client.publish("unity_P5", send_message);
    // memset(send_message, 0, sizeof(send_message));

    if(is_Lock == 1)
    {

      for (int i = 0; i < 5; i++)
      {
          position_array[i] = LobotSerialServoReadPosition(MySerial0,i+1);
      }
      
      // printf("position_array[1]=%d\r\n",position_array[0]);
      // printf("position_array[2]=%d\r\n",position_array[1]);
      // printf("position_array[3]=%d\r\n",position_array[2]);
      // printf("position_array[4]=%d\r\n",position_array[3]);
      // printf("position_array[5]=%d\r\n",position_array[4]);
      //P1
      position_array[0] = (int)(((float)(position_array[0]-487))/4.17)*(-1);
      sprintf(send_message, "%d", position_array[0]);
      client.publish("unity_P1", send_message);
      memset(send_message, 0, sizeof(send_message));
      // printf("position_array[1]=%d\r\n",position_array[0]);
      //P2
      position_array[1] = (int)(((float)(position_array[1]-500))/4.17)*(-1);
      sprintf(send_message, "%d", position_array[1]);
      client.publish("unity_P2", send_message);
      memset(send_message, 0, sizeof(send_message));
      // printf("position_array[2]=%d\r\n",position_array[1]);
  
      //P3
      position_array[2] = (int)(((float)(position_array[2]-500))/4.17);
      sprintf(send_message, "%d", position_array[2]);
      client.publish("unity_P3", send_message);
      memset(send_message, 0, sizeof(send_message));
      // printf("position_array[3]=%d\r\n",position_array[2]);
  
      //P4
      position_array[3] = (int)(((float)(position_array[3]-500))/4.17);
      sprintf(send_message, "%d", position_array[3]);
      client.publish("unity_P4", send_message);
      memset(send_message, 0, sizeof(send_message));
      delay(40);


      for (int i = 0; i < 5; i++)
      {
          temperature_array[i] = LobotSerialServoReadTemp(MySerial0,i+1);
      }
     
      sprintf(send_message, "%d", temperature_array[0]);
      client.publish("unity_st1", send_message);
      memset(send_message, 0, sizeof(send_message));
      //printf("temperature_array[0]=%d\r\n",temperature_array[0]);
  
      sprintf(send_message, "%d", temperature_array[1]);
      client.publish("unity_st2", send_message);
      memset(send_message, 0, sizeof(send_message));
      //printf("temperature_array[1]=%d\r\n",temperature_array[1]);
  
      sprintf(send_message, "%d", temperature_array[2]);
      client.publish("unity_st3", send_message);
      memset(send_message, 0, sizeof(send_message));
      //printf("temperature_array[2]=%d\r\n",temperature_array[2]);
  
      sprintf(send_message, "%d", temperature_array[3]);
      client.publish("unity_st4", send_message);
      memset(send_message, 0, sizeof(send_message));
      //printf("temperature_array[3]=%d\r\n",temperature_array[3]);
  
      sprintf(send_message, "%d", temperature_array[4]);
      client.publish("unity_st5", send_message);
      memset(send_message, 0, sizeof(send_message));
      //printf("temperature_array[4]=%d\r\n",temperature_array[4]);
      delay(40);
  
      for (int i = 0; i < 5; i++)
      {
          volatge_array[i] = LobotSerialServoReadVin(MySerial0,i+1);
      }
  
      sprintf(send_message, "%.03f", (float)volatge_array[0]*0.001);
      client.publish("unity_sv1", send_message);
      memset(send_message, 0, sizeof(send_message));
  
      sprintf(send_message, "%.03f", volatge_array[1]*0.001);
      client.publish("unity_sv2", send_message);
      memset(send_message, 0, sizeof(send_message));
  
      sprintf(send_message, "%.03f", volatge_array[2]*0.001);
      client.publish("unity_sv3", send_message);
      memset(send_message, 0, sizeof(send_message));
  
      sprintf(send_message, "%.03f", volatge_array[3]*0.001);
      client.publish("unity_sv4", send_message);
      memset(send_message, 0, sizeof(send_message));
  
      sprintf(send_message, "%.03f", volatge_array[4]*0.001);
      client.publish("unity_sv5", send_message);
      memset(send_message, 0, sizeof(send_message));
      delay(20);
  
      LobotSerialServoMove(MySerial0,1,value_1,2000);
      LobotSerialServoMove(MySerial0,2,value_2,2000);
      LobotSerialServoMove(MySerial0,3,value_3,2000);
      LobotSerialServoMove(MySerial0,4,value_4,2000);
      LobotSerialServoMove(MySerial0,5,value_5,2000);
      delay(1800);

      setClock();

    }

}





